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Improving Terrain Navigation by Concurrent 
Tidal and Sound Speed Error Estimation 


Abstract—Terrain navigation in the presence of an unknown 
tide level has been demonstrated earlier, where both horizontal 
position and depth errors of a nominal navigation solution can 
be estimated concurrently. Accurate bathymetric measurements 
based on acoustic sensors also rely on knowing the sound speed 
near the sensor and in the water column between the sensor 
and the sea floor. This is important for vessels operating far 
from the sea floor, but also for underwater vehicles operating 
near the sea floor with side-looking bathymetric systems. To 
enable high performance terrain navigation in those scenarios, 
the sound speed error must also be considered. This paper 
proposes three different modifications to a 2D point mass filter to 
enable concurrent compensation for tide and sound speed error. 
Simulations so far indicate a great improvement compared to 
using a regular 2D point mass filter. 


I. INTRODUCTION 


Terrain navigation is a technique increasingly used for 
subsurface position updates of Autonomous Underwater Vehi- 
cles (AUVs). By comparing bathymetric measurements with 
a digital terrain model (DTM), the best matching position 
may be used to update an aided inertial navigation system 
(INS). This efficiently bounds the position error of the AUV 
in suitable terrain, and several real-time systems have been 
developed to demonstrate the concept [1]-[4]. The technique 
has also been proposed and demonstrated as a technique for 
making surface vessels robust against GPS jamming- and 
spoofing attacks [5]. Common to both these scenarios is the 
use of acoustic sensors to measure the bathymetry. Accurate 
bathymetric measurements with acoustic sensors rely on know- 
ing the sound speed through the water column between the 
sensor and the sea floor. Error in sound speed, both near the 
sensor and in the water column, together with inaccuracies in 
the ray propagation algorithm, results in errors, both in the 
estimated depth and in the location of the footprint on the 
sea floor. Usually a surface vessel is more susceptible to these 
errors, but there are cases for AUVs, even at low altitude, when 
sound speed errors are important [6]. This is especially true 
when using side-looking sonars with grazing angles, at which 
refraction of the acoustic signal increases. Another common 
error is due to the difference in the vertical datum of the DTM 
and the real-time bathymetric measurements. The DTM may 
be compensated for the tide level in post-processing, but in 
real-time the AUV and the surface vessel usually only know 
the predicted tide level, or simply refer the measured depth to 
current sea level. Fig. 1 gives an example of what is likely a 
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Fig. 1. Mean profile difference between 2.5 minutes of EM710 real-time 


measurements and a DTM based on post-processed EM1002 measurements 
(black) as a function of beam number, along with the standard deviation 
(broken blue). 


combination of sound speed and tidal error when comparing 
real-time EM710 multibeam echo sounder measurements at 
the true position of the surface vessel H.U. Sverdrup II with a 
DTM constructed from post-processing an earlier survey with 
an EM1002 multibeam echo sounder [5]. Most of the bias in 
the profile is caused by the unknown tide level, but a smaller 
part of the bias and the curvature of the profile is most likely 
caused by a sound speed error. 

In the most suitable terrain types, the terrain navigation 
algorithms converge even in the presence of this type of 
error, but in more suitable terrain the error may influence the 
convergence of the algorithms. This paper explores the idea of 
using the information in the difference profiles, as shown in 
Fig. 1, to concurrently estimate and compensate for the sound 
speed and tidal errors. Three approaches to integration of the 
sound speed and tidal error bias estimates with the point mass 
filter (PMF) terrain navigation algorithm are considered: 


e Correct the measurements with the most likely bias 
estimates based on the last good position fix. 

e Use an improved covariance model in the PMF estimator. 

e Use a Rao-Blackwellized version of the PMF estimator. 


The three new methods are compared to the relative profile 
method [7], amongst others used in the PMF implemented in 


the HUGIN AUV terrain navigation system today [4]. Section 
II reviews the Bayesian formulation of terrain navigation, 
and the point mass filter estimator. An error model for the 
sound speed and tidal errors is developed, and three modified 
versions of the PMF are introduced. Simulations from two 
scenarios are used to test and compare the modified versions 
in Section HI. Finally some preliminary conclusions and 
suggestions for future work are given in Section IV. 


Il. TERRAIN NAVIGATION 
A. Dynamic model 


To enable practical calculation for the nonlinear terrain 
navigation problem, a simplified mathematical model for the 
dynamics of the surface vessel and the measurements of the 
bathymetric sensors is presented. Let £k = (£k, Yk, Zk) denote 
the vessel’s position in a local north-east-down system at time 
t = tx. Consider the following dynamical model for the 
position states of the vessel 


LE41 = Le + UR + Ve, (1) 


where ux is the integrated motion estimated by the INS in the 
time interval t to t,41, and vk is a white stochastic process 
modeling the error drift in the INS estimate. 


B. Measurement model 


Let (EN, ni, Ck) denote mp bathymetric depth measure- 
ments relative to the INS solution in a local level north 
aligned system N, and let h(x) = h(x, y) denote the terrain 
function, which gives the terrain depth as a function of global 
horizontal position. We consider the following model for the 
measurements 


Ck = herm (£k) + We- (2) 


Here we have introduced the measurement vector function 
hé, m, (£k) = {hi}; with scalar components defined by 


hi = h(£k + EN Yk + hi) — Rhe — Zk + Gk, (3) 


and wz, is a white stochastic process modeling the bathymetric 
measurement error. This particular form of the measurement 
function is called relative profile matching, where the average 
of the measured profile C, (including vessel depth), and the 
corresponding average terrain profile hy, have been subtracted 
to eliminate potentially large unknown biases, e.g. due to tides. 
If Ck = hy = 0 the measurement function is called absolute 
profile matching. 

Consider now a swath bathymeter, e.g. a multi beam echo 
sounder (MBE) or an interferometric sidescan sonar, where the 
measurement referenced in a local level vehicle body system B 
becomes (0, në ,¢x), assuming the measurements have been 
compensated for roll and pitch from the INS. If the vehicle 
moves directly forward, the measurements form a fan in the 
across track direction. The fan can be parameterized by a beam 
angle of the vertical, ¢;, and a one-way propagation time, 7;, 


for the acoustic signal. The beam angle is estimated from angle 
of arrival estimation, and depends on sound speed as [8] 
ai antes) fas 
c c 
and 7; is estimated from the bottom detection signal processing 
of the bathymeter. Here we only consider the case with 
constant sound speed throughout the water column. The vector 
of beam 7 from the body system to a point on the sea floor S, 
decomposed in NV is then given by 








0 
P3si=Rne en sin alo (5) 
cT; cos Q; (c) 


Ryg = le$, ehy eJ] denotes the rotation matrix from B 


to N, a simple rotation about the vertical axis compensating 
for the vehicle’s heading. By using the second and third unit 
column vectors of Ryp this can be written as 


PEs ij =cT; (sin bi(C)eR y + cos di(c)es -) ; (6) 
Finally, on component form the measurement depth of the 
acoustic bathymeter is given by 


Cki = DES i f en. = cr; cos Q; (c), (7) 
and the horizontal footprint component is given by 
Nha = DES 4 . eR y = cr; sin d;(c). (8) 


The corresponding expected depth measurement from the 
terrain function is then given by 


hki = h(£k + Tk i€B y) Zi (9) 
C. Point mass filter estimator 


The algorithm used in the HUGIN terrain navigation system 
[4] is based on the PMF, a nonlinear Bayesian estimator 
that estimates the probability density function (PDF) of the 
state vector on a grid [9], [10]. It was first used in aircraft 
terrain navigation [11], but has later made its way into under- 
water terrain navigation. Before introducing the actual PMF 
approximation, we consider the Bayesian formulation of the 
estimation problem (1) and (2). 

We define a 2D bounded domain G in the earth tangent 
plane, enclosing the current INS solution. Let 6a; denote a 
position vector in Gw (the error of the INS) with reference 
to a north-aligned system with origin at the INS solution, 
and let Z; denote the set of all measurements between tı 
and t. Our goal is to estimate the position error filter PDF 
p(ô£k|Zk), conditioned on all the measurements so far in the 
correlation period |to,t,]. From (1) we find that da, is a 
Markov process, and its PDF evolves in time according to the 
convolution integral [12] 


POATE J pos (Sate 1 — 6eby)p(Sary|Z)d6ar5, (10) 
Gk 


where p,,(-) = p(ô£k+1|0£p) is the Markovian transition 
kernel for the error drift. 
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(a) Position error PDF after 1 ping 
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(b) Position error PDF after 10 pings 


Fig. 2. Example of the position error PDF estimated by PMF after 1 (a) and 10 (b) pings, using only three beams from an EM710 multibeam echo sounder 
and 1 minute between each ping. True position is still recovered 100 m both south and west of the estimated position of the INS, which is always in the 


center of the 800 m x 800 m grid of the PMF. 


The measurement update, following Bayes’ theorem, is 
given by 
p(dary| Zp) = Pe ecn e + AEE E Ee, 

i (11) 
where a, = Je, Pwr (Ck = hrn (Tk + ÔLk))pló£k|Zk)dóE£k 
is a normalization constant, and pw, (-) denotes the PDF for 
the sensor measurement error. Together the equations (10) 
and (11) form the recursive Bayesian estimator equations for 
terrain navigation. The recursion is started by a known initial 
PDF p(6a9|Zo) = po(dap) at to that depends on the initial 
INS accuracy, and the filter then runs recursively for each ping 
until a convergence or divergence criterion is met at tg. The 
PMF approximation is found by representing the filter PDF 
in (10) and (11) by point masses on a regularly spaced grid 
representation of Gg, and by calculating the integrals through 
straightforward quadrature. Once the PMF approximation of 
the PDF is found, estimates of the INS position error and 
the accuracy of this estimate can be calculated directly from 
the approximated PDF [12]. Fig. 2 shows an example of the 
evolution of the PDF in the correlation period. 





D. Tide and sound speed error model 


To extend the PMF algorithm of terrain navigation to 
include concurrent modeling and estimation of tide and sound 
speed error, we will develop a first order linear model for the 
error. The Taylor series of the measurement (7) is given by 

















2 Cki Cki 
Cki = Cea + Gk, c + Sr, E +... (12) 
Oc Oz 
From (7) 3$ = 0 and 
okki anA Oka Tea \ Cki 
ese i) — = |1 a 1 
J (1 — tan? ¢;) m o) e (13) 


The corresponding Taylor series of the measurement model 
(9) is given by 




















A Ohk, i Ohk i 

hri = hpi + 260+ "dx, +... (14) 
i , Oc zk 

From (9) u = —1 and 

Ok i Nki N 

Z = Vh. : : 15 

Oc Oc Bw (15) 

The footprint dependency on sound speed is found from (8) 
ONk i ; Od; i 
"ik, = Ti sin Q; + CT; COS p a = 2k: š (16) 
Oc Oc c 


Finally, we can rewrite the measurement model equation (2) 
with a linear tide and sound speed error model given by 


Cx = he, (an) + Hke + wp- (17) 


Here we have introduced the environment error state vector 
T : 
ek = [6z, dc],, and the Mmg x 2 matrix 


Hix=|-1 2&va-e¥, — (1- 2% | 2], 
c i ik) © 





(18) 


which projects the environment errors onto a corresponding 
depth error. 


E. Modified PMF algorithms 


The state of the estimation problem is now augmented by 
sound speed and depth errors, [da, €], making the problem four 
dimensional. The computational complexity of using a regular 
PMF directly on this problem is too high to be feasible for real- 
time applications [7], although a particle filter implementation 
could be considered [7], [13]. We instead consider three 
different modifications to the regular PMF. 


1) PMF with General Least Square Estimate: Assume that 
the filter has converged to the true position æ. By defining 
the residual profile by dy = Cx — he, m,(x) the goal is to 
solve the estimation problem 


dg = Hye, + we. (19) 


We denote the measurement covariance matrix by Ry = 
E|w;,w;|, which now includes both the bathymetric sensor 
errors and the DTM errors. The analytic solution of the general 
linear least square (GLS) estimate is then given by [14] 


z =T = 
ek = (Hi R} 'H,) Hi R,'d; (20) 


The estimate is unbiased and asymptotically normally dis- 
tributed with covariance given by 


1 


P.,, = Elexe;] = (HE RZ `H) (21) 


The modified version of the PMF is based on relative profile 
matching, and the GLS estimation of the sound speed and 
tide error is only done after the regular PMF has converged to 
an accurate position estimate. The estimated error are then 
compensated for subsequently in order to improve terrain 
navigation performance in less suitable terrain. These mod- 
ififications to the algorithm is referred to as PMF-GLS. 

2) PMF with sound speed and tide error model: This 
version is not really a modification of the regular relative 
profile matching PMF, but rather improves the error model 
by augmenting it with the tide and sound speed error model 
in (17), leading to a total error given by 


Wi, = Hye, + we. (22) 


The term wą only models DTM error and uncorrelated sensor 
errors. The modified covariance is then given by 


Ri, = HP., HF + Rx. (23) 
This modified PMF algorithm is referred to as PMF-MOD. 
F. Rao-Blackwellized PMF 


Since the dynamic equation (constant tide and sound speed 
in the correlation period) and the measurement model is 
linear in the sound speed and depth error subspace containing 
€k, and the measurement and dynamic noise is modeled as 
Gaussian, the estimation problem can be solved by a Rao- 
Blackwellization of the PMF estimator. This technique has 
earlier been done in other estimation problems for particle 
filters [15]. By Bayes’ theorem the joint PDF for the position 
and environmental errors is given by 


P(ÔTk, Ek|Zk) = p(ôTk|Zk)plEk|ðtk, Zk). 


The first PDF factor is found using the PMF estimator in 
absolute profiling mode, and the second PDF factor can be 
optimally found using Kalman filters conditioned on each grid 
point of the PMF. Effectively, at each grid point of the 2D 
position error in the regular PMF, a Kalman filter is then 
maintained for the sound speed and depth error given the 
corresponding 2D position error. However, in cases of low 
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Fig. 3. Tracks for the two simulation scenarios overlaid the DTM: the flat 
terrain scenario (red), and the sloping terrain scenario (yellow). 


gradient sea floors the first term in the second column of 
(18) can be ignored, and both the dynamic model and the 
linearized measurement model of the augmented subspace 
does not depend on the particular grid point. This means that 
a common Kalman gain K,, and covariance matrix Pe,, is 
valid for the sound speed and tide error at each grid point. 
The analytical Kalman filter solution for grid point 7 is then 
given by [16] 


€k,j = €k-1,5 + Kp (Ck — hex m (Ek + 0Ek j) — Hex) 


P.,, = (I — Ky Hx) Pap; (25) 
= 
Ky = Pe, HE (HiPe,,Hi + Re). 


This modified PMF algorithm is referred to as PMF-RB. 


II. SIMULATION RESULT 


To get indications of performance of the algorithms, some 
simulations were performed and analyzed. The simulations 
consider a vessel traveling along at straight line at 2 m/s, 
carrying an MBE with a swath width of 130°. The algorithms 
use only 13 beams (uniformly spread in the swath width) from 
the MBE every third second. 

The underlying terrain used in the simulations is based on a 
real DTM constructed from a survey outside Horten in the 
Oslo fjord. This DTM is considered as the true terrain, and 
another DTM was constructed based on adding white Gaussian 
noise with a standard deviation of 30 cm to the grid nodes of 
the true DTM. This simulated DTM was the one known to the 
algorithms. 

The environmental errors were simulated by choosing a fixed 3 
m tide level, while the algorithms initially assumed a nominal 
zero level. In addition a fixed 3 m/s of sound speed error was 
added to the nominal value of 1500 m/s, initially assumed 
by the algorithms. The measurement from the MBE was 
simulated by first computing a new unit beam vector based 
on the sound speed error and by using (4). The intersection 
between the new unit vector and the true DTM was then found, 
and used to calculate the range to the sea floor given this sound 
speed error. An additional white Gaussian error with standard 
deviation of 15 cm was finally added to the estimated range 
to the sea floor. 

All the PMF algorithms were configured to use a 200 m 


TABLE I 
FLAT TERRAIN SCENARIO PERFORMANCE 




















th East Depth d Spd 

Algorithm Error Nor = ep Spee 
[m] [m] [m] [m/s] 
PMF Mean -26.07 | -27.89 -0.06 n/a 
Std Dev | 25.21 68.75 0.12 n/a 
PMF-MOD Mean 0.25 -1.23 0.15 n/a 
Std Dev 2.93 2.67 0.07 n/a 

PMF-GLS Mean 1.64 0.10 -0.03 -0.01 
Std Dev 3.93 4.76 0.02 0.16 

PMF-RB Mean 1.39 0.08 -0.01 0.04 
Std Dev 2.26 1.81 0.02 0.08 


























by 200 m grid with 1 m resolution. The initial PDF was 
chosen to be the uniform distribution, while the dynamic 
noise PDF was chosen to be a Gaussian with covariance 
based on 10 cm/s drift. Finally the measurement noise was 
modeled by a Gaussian combining the MBE noise and the 
DTM noise levels. The final covariance of the Gaussian used 
in the PMF estimator also includes the tide and sound speed 
error, as described for the different algorithms in Section II-E. 
The PMF algorithms all used the same set of convergence 
criterions for the approximated PDF. The criterions are based 
on a single modality measure, the similarity of the PDF to a 
Gaussian measured by the Hellinger distance, and the amount 
of information in the PDF measured by the Kullback-Leibler 
divergence [6]. 

Two scenarios were simulated. In the first scenario the vessel 
travels over flat parts of the terrain, and in the second over 
sloping parts of the terrain with some additional variations, 
see Fig. 3. 


A. Flat terrain 


This scenario lasts for 615 s, and the mean measured water 
depth is about 195 m. The terrain is mainly flat, but the swath 
with is about 800 m wide, so the MBE actually registers some 
variation in the outermost beam. The errors of the position and 
environmental states from all the algorithms are plotted in Fig. 
4, and its mean and standard deviation are given in Table I. 
The regular PMF gets a few false position fixes, but diverges 
for most of the time. The false fixes are caused by the fact 
that the measurement noise occasionally correlates with the 
DTM noise in an otherwise flat terrain. In the HUGIN terrain 
navigation system [4] this would be detected and rejected by 
its integrity system. It is interesting and surprising that PMF- 
MOD is able to counter the false fixes and performs well 
simply through improved modeling, although not increasing 
the amount of position fixes to any degree. The PMF-GLS 
and PMF-RB algorithms get the highest amount of position 
fixes, which are all with highly accurate. They are also able 
to estimate both tide level and sound speed accurately in this 
scenario. 


TABLE II 
SLOPING TERRAIN SCENARIO PERFORMANCE 

















North | East | Depth d Spd 

Algorithm Error pa as ep Snd'Sp 
[m] [m] [m] [m/s] 
PMF Mean -4.48 4.47 0.84 n/a 
Std Dev 4.36 4.93 0.33 n/a 
PMF-MOD Mean -0.75 0.81 0.31 n/a 
Std Dev 1.59 2.06 0.19 n/a 
PMF-GLS Mean -0.24 0.22 0.06 0.19 
Std Dev 2.01 2.61 0.33 0.65 
PMF-RB Mean 0.20 0.01 -0.01 0.03 
Std Dev 1.30 1.43 0.16 0.41 


























B. Sloping terrain 


This scenario lasts for 687 s, and the mean measured water 
depth is about 140 m. The terrain is sloping with some 
additional variation to it. The swath width is about 600 m 
wide. The errors of the position and environmental states from 
all the algorithms are plotted in Fig. 5, and its mean and 
standard deviation are given in Table II. All the algorithms get 
a comparable amount of position fixes in this scenario. The 
PMF algorithm converges for all fixes but with a bias in the 
south-east direction, which coincides with the direction of the 
slope. This bias is caused by the fact that the algorithm is not 
able to estimate the tide level accurately enough. The PMF- 
MOD performs substantially better with only a corresponding 
bias of less than 1 m. The PMF-GLS and PMF-RB algorithms 
perform even better, and again they are able to estimate both 
the tide level and the sound speed accurately. 


IV. CONCLUSION AND FUTURE WORK 


Three different modifications to the regular PMF algorithm 
have been developed to enable concurrent estimation and 
compensation for tide and sound speed errors in terrain 
navigation. All three algorithms have been shown to perform 
well in simple simulation scenarios for flat and sloping terrain. 
Most surprisingly the simulations show a great performance 
increase in just improving the measurement model as in 
the PMF-MOD algorithm, but in both the scenarios the 
algorithms that concurrently also estimated both the tide 
and sound speed, the PMF-GLS and PMF-RB algorithms, 
performed better. 

The simulations should be improved by including sound 
speed variations through the water column. And for improved 
assessment of the robustness and performance, Monte Carlo 
simulations of the scenarios should be done. 

The real test is however performance on real data from both 
surface vessel and AUV. 
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Fig. 5. Sloping terrain scenario performance for the regular PMF in (a), the PMF with added environmental modeling in (b), the PMF with decoupled linear 
least square environment estimation in (c) and the Rao-Blackwellized PMF in (d). The position and environment state errors (blue), along with estimated 
standard deviation 1o (green) and 3c (broken green). 


